/*************************************************** 
 This is a library for our Adafruit 16-channel PWM & Servo driver

 Pick one up today in the adafruit shop!
 ------> http://www.adafruit.com/products/815

 These displays use I2C to communicate, 2 pins are required to
 interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4

 Adafruit invests time and resources providing this open source code,
 please support Adafruit and open-source hardware by purchasing
 products from Adafruit!

 Written by Limor Fried/Ladyada for Adafruit Industries.
 BSD license, all text above must be included in any redistribution
 ****************************************************/

#include <pca9685.h>
#include <stdlib.h>
#include "math.h"

// Set to true to print some debug messages, or false to disable them.
#define ENABLE_DEBUG_OUTPUT true

PWMServoDriver::PWMServoDriver(I2C_HandleTypeDef *_p_i2c, uint8_t addr) {
	p_i2c = _p_i2c;
	i2caddr_write = addr;
}


void PWMServoDriver::reset(void) {
	write8(PCA9685_MODE1, 0x0);
}

void PWMServoDriver::setPWMFreq(float freq) {
	//Serial.print("Attempting to set freq ");
	//Serial.println(freq);
	freq *= 0.9; // Correct for overshoot in the frequency setting (see issue #11).
	// PCA9685模块内晶振时钟是 25M
	float prescaleval = 25000000;
	prescaleval /= 4096;
	prescaleval /= freq;
	prescaleval -= 1;


	uint8_t prescale = floor(prescaleval + 0.5);
	//标准计算结果是121，实际频率是41hz
	// 50hz
	prescale = 0x73;
	//49hz, 7b=123
	//prescale = 0x7B;



	// mode1下设置sleep=1， 才能改写prescale
	uint8_t oldmode = read8(PCA9685_MODE1);
	uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep
	write8(PCA9685_MODE1, newmode); // go to sleep
	write8(PCA9685_PRESCALE, prescale); // set the prescaler

	// 退出sleep状态
	oldmode = read8(PCA9685_MODE1);
	uint8_t t2 = oldmode & 0x80 ;
	if( t2 != 0 )
	{
		// bit7 =1, 处于restarting状态， 要重启pwm channel
		uint8_t t3 = oldmode&0xEF;
		write8(PCA9685_MODE1, t3);
		HAL_Delay(1000);
	}
	else
	{
		// 不处于restarting状态，bit4=1， 处于sleep状态
		oldmode = read8(PCA9685_MODE1);
		uint8_t t4 = oldmode & 0x10 ;
		if (t4 != 0)
		{
			write8(PCA9685_MODE1, oldmode&0xEF);
		}

	}



	// 这一步真的启动。编译器有问题， oldmode值会被修改掉，重用时，数据不对了。
	/*bit7 =1
	 * bit6 =0
	 * bit5 =1, AI auto increment
	 * bit4 =0, sleep
	 * bit3 =0
	 * bit2 =0
	 * bit1 =0
	 * bit0 =1 , response to all call
	*/
	oldmode = read8(PCA9685_MODE1);
	write8(PCA9685_MODE1,  oldmode|0xa1);
	HAL_Delay(100);

}

void PWMServoDriver::setPWM(uint8_t num, float angle, float servo_max_angle)
{
	//看舵机参数，有效脉冲宽度是500 ~ 2500us。 中点是1500us, 2500us-500us= 2000us
	uint16_t pulse_width_us = (uint16_t) (500 + (angle / servo_max_angle) * 2000);

	// 将脉冲宽度转换为步进数 (PCA9685 的分辨率为 12 位，即 4096 步) . 这里的20000us= 20ms*1000
	uint16_t step = (uint16_t) (pulse_width_us/(20*1000.0f) * 4095);

	uint8_t data[4];
	// 0表示输出在周期开始时立即打开。
	data[0] = 0x00; // ON_L
	data[1] = 0x00; // ON_H
	//在此时刻开始的点，输出关闭
	data[2] = step & 0xFF; // OFF_L
	data[3] = step >> 8;   // OFF_H

	//写入数据高低反了，干脆逐个字节写入。
	/*
	HAL_I2C_Mem_Write(p_i2c, i2caddr_write, LED0_ON_L+ 4 * num, 1,  &data[0], 1, HAL_MAX_DELAY);
	HAL_I2C_Mem_Write(p_i2c, i2caddr_write, LED0_ON_H+ 4 * num, 1,  &data[1], 1, HAL_MAX_DELAY);
	HAL_I2C_Mem_Write(p_i2c, i2caddr_write, LED0_OFF_L+ 4 * num, 1, &data[2], 1, HAL_MAX_DELAY);
	HAL_I2C_Mem_Write(p_i2c, i2caddr_write, LED0_OFF_H+ 4 * num, 1, &data[3], 1, HAL_MAX_DELAY);
	*/
	write8(LED0_ON_L+ 4 * num, data[0]);
	write8(LED0_ON_H+ 4 * num, data[1]);
	write8(LED0_OFF_L+ 4 * num, data[2]);
	write8(LED0_OFF_H+ 4 * num, data[3]);


}


uint8_t PWMServoDriver::read8 (uint8_t addr)
{
	uint8_t result = 0;
	if (HAL_I2C_Mem_Read(p_i2c, i2caddr_write | 0x01, addr, 1, &result, 1, 1000)
			== HAL_OK)
	{
		return result;
	}
	else
	{
		return -1;
	}

}

void PWMServoDriver::write8(uint8_t addr, uint8_t d) {
	// I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size, uint32_t Timeout
	HAL_StatusTypeDef result =HAL_I2C_Mem_Write(p_i2c, i2caddr_write, addr, 1, &d, 1, 1000) ;
	bool is_success = false;
	if ( result == HAL_OK)
	{
		is_success = true;
	}
	else
	{
		is_success = false;
	}
	HAL_Delay(10);
}
